#include <msp430x26x.h>
#include "../Hardware/PortDef.h"
#include "../Utilities/Delay.h"
#include "../Communication/SPI.h"
#include "Mag.h"

// Special Mag control MACROs
#define PULSE_RESET() \
	{\
		P2OUT |= MagRst;\
		DELAY_250NS();\
		P2OUT &= ~MagRst;\
	}
#define MAG_RDY_BIT_NUM (1)
#define DATA_READY  	((P2IN & MagDatRdy) >> MAG_RDY_BIT_NUM)
// Absolute value macro
#define ABS(x) (x > 0 ? x : -x)

// Control command defines
#define CONVERT_X 0x31
#define CONVERT_Y 0x32
#define CONVERT_Z 0x33

// Private constants
const unsigned int ratio_table[18] = {0,22,45,68,93,119,147,179,214,256,305,365,443,548,703,955,1451,2926};
const unsigned int deg_table[18] = {0,5,10,15,20,25,30,35,40,45,50,55,60,65,70,75,80,85};

// Private variables
static int low, high, index, angle;
static long direction;

// Private functions
static void sendCommand(unsigned char command);
static int getValue(void);

/*
 * Initialize the magnetometer.  This involves pulsing the reset line (setting it on
 * for about 250ns and then off) while enabling the magnetometer.  Then delaying
 * for about 250ns.  Extra delays are placed in the algorithm for safety to
 * ensure a proper reset.
 */
void MagInit(void)
{
	ENABLE_MAG();
	DELAY_250NS();
	PULSE_RESET();
	DELAY_250NS();
	DISABLE_MAG();
	DELAY_250NS();
}

/*
 * Get the magnetic field strength along the X-Axis.  Enable the mag, wait a
 * bit (250ns), pulse the reset pin, delay some more, sent the convert x command
 * and wait for the data to be ready (DATA_READY pin goes high).  Then clock
 * out the converted value and disable the mag.
 */
int MagGetX(void)
{
	int temp;
	
	ENABLE_MAG();
	DELAY_250NS();
	PULSE_RESET();
	DELAY_250NS();
	sendCommand(CONVERT_X);
	while (!DATA_READY);
	temp = getValue();
	DISABLE_MAG();
	DELAY_250NS();
	
	return temp;
}

/*
 * Get the magnetic field strength along the Y-Axis.  Enable the mag, wait a
 * bit (250ns), pulse the reset pin, delay some more, sent the convert y command
 * and wait for the data to be ready (DATA_READY pin goes high).  Then clock
 * out the converted value and disable the mag.
 */
int MagGetY(void)
{
	int temp;
	
	ENABLE_MAG();
	DELAY_250NS();
	PULSE_RESET();
	DELAY_250NS();
	sendCommand(CONVERT_Y);
	while (!DATA_READY);
	temp = getValue();
	DISABLE_MAG();
	DELAY_250NS();
	
	return temp;
}

/*
 * Get the magnetic field strength along the Z-Axis.  Enable the mag, wait a
 * bit (250ns), pulse the reset pin, delay some more, sent the convert z command
 * and wait for the data to be ready (DATA_READY pin goes high).  Then clock
 * out the converted value and disable the mag.
 */
int MagGetZ(void)
{
	int temp;
	
	ENABLE_MAG();
	DELAY_250NS();
	PULSE_RESET();
	DELAY_250NS();
	sendCommand(CONVERT_Z);
	while (!DATA_READY);
	temp = getValue();
	DISABLE_MAG();
	DELAY_250NS();
	
	return temp;
}

/*
 * Convert an X-Y field strength reading into a heading (0-355 degrees).
 */
int MagGetDirection(int x, int y)
{
	// This is from multipoint magnetometer calibration (see paper and wiki)
	x -= 63;					// subtract out the offsets 
	y += 33;
	x = ((long)x * 415) / 326;	// perform gain matching
	
	// Make sure x is not zero (don't want to divide by 0)
	if (x != 0)
	{
		// Calculate direction ratio and binary search for the angle
		direction = (((long)(ABS(y))) << 8) / (((long)(ABS(x))));
		low = 0;
		high = 17;
		index = 8;
		while(high > low)
		{
			if (ratio_table[index] < direction)
			{
				low = index + 1;
				index = ((high - low) >> 1) + low;
			}
			else
			{
				high = index - 1;
				index = ((high - low) >> 1) + low;
			}
		}
		
		// Cap the index at 0 and 17 to be sure
		if (index < 0)
		{
			index = 0;
		}
		else if (index > 17)
		{
			index = 17;
		}
		
		// Found matching ratio, get the angle (make sure we get the closest angle)
		if (direction < ratio_table[index] && index > 0)
		{
			if ((ratio_table[index-1] - direction) < (direction - ratio_table[index]))
			{
				angle = deg_table[index-1];
			}
			else
			{
				angle = deg_table[index];
			}
		}
		else if (direction > ratio_table[index] && index < 17)
		{
			if ((ratio_table[index+1] - direction) < (direction - ratio_table[index]))
			{
				angle = deg_table[index+1];
			}
			else
			{
				angle = deg_table[index];
			}
		}
		else
		{
			angle = deg_table[index];
		}
		
		// Now correct the angle for quadrant
		if (x < 0 && y >= 0)
		{
			angle = 180 - angle;
		}
		else if (x < 0 && y < 0)
		{
			angle = 180 + angle;
		}
		else if (x > 0 && y < 0)
		{
			angle = 360 - angle;
		}
	}
	else
	{
		// x is zero, angle calculation is easy
		if (y < 0)
		{
			angle = 270;
		}
		else
		{
			angle = 90;
		}
	}
	
	// Return the heading angle we just calculated
	return angle;
}

/*
 * Send an arbitrary character over SPI.  This is manually bitbanged SPI
 * according to the Mag datasheet specification.
 */
static void sendCommand(unsigned char command)
{
	unsigned char i;
	
	for (i = 8; i > 0; i--)
	{
		SCLK_LOW();
		DELAY_250NS();
		if (command & 0x80) // MSB first
		{
			MOSI_HIGH();
		}
		else
		{
			MOSI_LOW();
		}
		SCLK_HIGH();
		DELAY_250NS();
		command <<= 1; // Put next bit in MSB
	}
}

/*
 * Receive a character over SPI.  This is manually bitbanged SPI
 * according to the Mag datasheet specification.
 */
static int getValue(void)
{
	unsigned int temp;
	unsigned int i;
	
	// Initialize data to zero
	temp = 0x0000;
	
	MOSI_LOW(); // 0's are placeholders
	
	for (i = 0x8000; i > 0; i>>=1)
	{
		SCLK_HIGH();
		DELAY_250NS();
		if (MISO)
		{
			temp |= i;
		}
		SCLK_LOW();
		DELAY_250NS();
	}
	
	return temp;
}
